


%%location (xyz)


%%calibration

angle1a=0;

angle1b=1.4;

angle1c=0;


angle2a=0;

angle2b=0.3782;

angle2c=1.7372;


angle3a=0;

angle3b=1;

angle3c=0;

%%coordinate(x,y,z)
%lego man points to .13
%{
x1=0.1;
y1=0.1;   %%change y from 13 to 11 make it move in toward arm
z1=0.1;

x2=0.1;
y2=-0.1;
z2=0.1;
%}

[x1 y1 z1]=Unit_Convert(1,1,4);
[x2 y2 z2]=Unit_Convert(5,1,5);
[x3 y3 z3]=Unit_Convert(1,2,5);
[x4 y4 z4]=Unit_Convert(2,5,1);
[x5 y5 z5]=Unit_Convert(6,9,2);
[x6 y6 z6]=Unit_Convert(6,10,2);







% x1=0.14;
% y1=0.14;   %%change y from 13 to 11 make it move in toward arm
% z1=-0.06;
% 
% x2=0.14;
% y2=-0.14;
% z2=-0.06;
% 
% 
% x3=0.21;
% y3=0.11;
% z3=-0.06;
% 
% x4=0.27;
% y4=0.045;
% z4=-0.06;
% 
% x5=0.175;
% y5=0.001;
% z5=-0.06;
% 
% x6=0.27;
% y6=-0.115;
% z6=-0.06;



x7=0.295;
y7=-0.125;
z7=-0.035;

x8=0.295;
y8=-0.125;
z8=-0.035;

x9=0.295;
y9=-0.125;
z9=-0.035;

x10=0.295;
y10=-0.125;
z10=-0.035;

%%angles for motors
[a b c]=inv_kin(x1,y1,z1);
display([a b c]);
%a=round(angle1a*a^2+angle1b*a+angle1c);
%b=round(angle2a*b^2+angle2b*b+angle2c);
%c=round(angle3a*c^2+angle3b*c+angle3c);
a=round(a*3.5);

b=round(b*2);  

c=round(c);
pt1=[a b c];
display([a b c]);


[a b c]=inv_kin(x2,y2,z2);
display([a b c]);

a=round(a*3.5);

b=round(b*2); 

c=round(c);
pt2=[a b c];
display([a b c]);

 [a b c]=inv_kin(x3,y3,z3);
a=round(a*3.5);

b=round(b*2); 

c=round(c);
pt3=[a b c];
 
 
 
% 
 [a b c]=inv_kin(x4,y4,z4);
a=round(a*3.5);

b=round(b*2); 

c=round(c);
 pt4=[a b c];
% 
[a b c]=inv_kin(x5,y5,z5);
a=round(a*3.5);

b=round(b*2); 

c=round(c);
 pt5=[a b c];
% 
 [a b c]=inv_kin(x6,y6,z6);
a=round(a*3.5);

b=round(b*2); 

c=round(c);
 pt6=[a b c];
% 
% [a b c]=inv_kin(x7,y7,z7);
% a=round(angle1a*a^2+angle1b*a+angle1c);
% b=round(angle2a*b^2+angle2b*b+angle2c);
% c=round(angle3a*c^2+angle3b*c+angle3c);
% pt7=[a b c];
% 
% [a b c]=inv_kin(x8,y8,z8);
% a=round(angle1a*a^2+angle1b*a+angle1c);
% b=round(angle2a*b^2+angle2b*b+angle2c);
% c=round(angle3a*c^2+angle3b*c+angle3c);
% pt8=[a b c];
% 
% [a b c]=inv_kin(x9,y9,z9);
% a=round(angle1a*a^2+angle1b*a+angle1c);
% b=round(angle2a*b^2+angle2b*b+angle2c);
% c=round(angle3a*c^2+angle3b*c+angle3c);
% pt9=[a b c];
% 
% [a b c]=inv_kin(x10,y10,z10);
% a=round(angle1a*a^2+angle1b*a+angle1c);
% b=round(angle2a*b^2+angle2b*b+angle2c);
% c=round(angle3a*c^2+angle3b*c+angle3c);
% pt10=[a b c];


%{
pt1=[-36 -13 73];
pt2=[0 0 0];
pt3=[-36 -13 73];
pt4=[0 0 0];
pt5=[-56 -95 116];
pt6=[0 0 0];
pt7=[-56 -95 116];
pt8=[0 0 0];
pt9=[-56 -95 116];
pt10=[0 0 0];
%}
%{
%%Inverse kinematics
startup_rvc
t = 0:2;
theta = [0, 0, 0];
d = [0.04, 0, 0];
a = [.067, .152, 0];
alpha = [-pi/2, 0, 0];
sigma = [0, 0, 0];
for i = 1:3
   
    L(i) = Link([theta(i), d(i), a(i), alpha(i)]);

end
robot_arm = SerialLink(L, 'name', 'Lego Bot');
robot_arm.tool = transl(0.190, -0.006, 0);
T1 = [ 1     0    0     .135;  %out radially
      0     1    0     .135;   %left right
      0     0    1      -.065; %up down
      0     0    0     1.0];
T2 = [ 1     0    0     0.31;  %out radially
      0     1    0     -0.152;   %left right
      0     0    1    0.0; %up down
      0     0    0     1.0];
Ts = ctraj(T1, T2, length(t));
mask = [1 1 1 0 0 0];
qs = robot_arm.ikine(Ts,[0 0 2], mask, 'alpha', .85, 'ilimit', 800);
pt1(1)=qs(3,1);
pt1(2)=qs(3,2);
pt1(3)=qs(3,3);

%}









%% Start up
h=COM_OpenNXT();
COM_SetDefaultNXT(h);


%% Do your stuff

myMotor1             = NXTMotor();   % create class
myMotor1.Port        = MOTOR_A;  % same as  myMotors.Port = 'BC';
myMotor1.Power       = -80;       % power range from -100 to 100
%myMotor1.TachoLimit  = 26+30;     % the number of degree of rotation  0 up to 9999 
myMotor1.SmoothStart = true;	% motor startup setting
myMotor1.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor1.SpeedRegulation = false;  % no regulation on speed

myMotor2             = NXTMotor();   % create class
myMotor2.Port        = MOTOR_B;  % same as  myMotors.Port = 'BC';
myMotor2.Power       = -80;       % power range from -100 to 100
%myMotor2.TachoLimit  = 75+20;     % the number of degree of rotation  0 up to 9999 
myMotor2.SmoothStart = true;	% motor startup setting
myMotor2.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor2.SpeedRegulation = false;  % no regulation on speed

myMotor3             = NXTMotor();   % create class
myMotor3.Port        = MOTOR_C;  % same as  myMotors.Port = 'BC';
myMotor3.Power       = 80;       % power range from -100 to 100
%myMotor3.TachoLimit  = 116;     % the number of degree of rotation  0 up to 9999 
myMotor3.SmoothStart = true;	% motor startup setting
myMotor3.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor3.SpeedRegulation = false;  % no regulation on speed

Power1=75;
Power2=60;
Power3=70;


for i=1:2
%pt1 conversion to machine command
if pt1(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt1(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt1(1);         
end  


if pt1(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt1(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt1(2);    
      
end 


if pt1(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt1(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt1(3); 
end 

display(pt1);

run_motor;

pt1(1)=-1*pt1(1);
pt1(2)=-1*pt1(2);
pt1(3)=-1*pt1(3);

if pt1(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt1(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt1(1);         
end  



if pt1(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt1(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt1(2);    
      
end 


if pt1(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt1(3); 

else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt1(3); 

end 

back_motor;

pt1(1)=-1*pt1(1);
pt1(2)=-1*pt1(2);
pt1(3)=-1*pt1(3);

display(pt1);

%{
step=(myMotor.TachoLimit)/3; 
step1=(myMotor1.TachoLimit)/3; 
step2=(myMotor2.TachoLimit)/3; 

myMotor.TachoLimit=round(step);
myMotor1.TachoLimit=round(step1);
myMotor2.TachoLimit=round(step2);

for i=1:3

    
myMotor.SendToNXT();
myMotor1.SendToNXT();
myMotor2.SendToNXT();
myMotor.WaitFor();
myMotor1.WaitFor();
myMotor2.WaitFor();
    
end   


%}




%pt2 conversion to machine command
if pt2(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt2(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt2(1);         
end  


if pt2(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt2(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt2(2);    
      
end 

if pt2(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt2(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt2(3); 
end 

run_motor;


pt2(1)=-1*pt2(1);
pt2(2)=-1*pt2(2);
pt2(3)=-1*pt2(3);

if pt2(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt2(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt2(1);         
end  


if pt2(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt2(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt2(2);    
      
end 

if pt2(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt2(3); 
myMotor3.TachoLimit=myMotor3.TachoLimit;
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt2(3); 
myMotor3.TachoLimit=myMotor3.TachoLimit;
end 

back_motor;

pt2(1)=-1*pt2(1);
pt2(2)=-1*pt2(2);
pt2(3)=-1*pt2(3);

display(pt2);


%pt3 conversion to machine command
if pt3(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt3(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt3(1);         
end  


if pt3(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt3(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt3(2);    
      
end 

if pt3(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt3(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt3(3); 
end 

run_motor;


pt3(1)=-1*pt3(1);
pt3(2)=-1*pt3(2);
pt3(3)=-1*pt3(3);

if pt3(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt3(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt3(1);         
end  


if pt3(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt3(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt3(2);    
      
end 

if pt3(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt3(3); 
myMotor3.TachoLimit=myMotor3.TachoLimit;
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt3(3); 
myMotor3.TachoLimit=myMotor3.TachoLimit;
end 

back_motor;

pt3(1)=-1*pt3(1);
pt3(2)=-1*pt3(2);
pt3(3)=-1*pt3(3);

display(pt3);

%pt4 conversion to machine command
if pt4(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt4(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt4(1);         
end  


if pt4(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt4(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt4(2);    
      
end 

if pt4(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt4(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt4(3); 
end 

run_motor;


pt4(1)=-1*pt4(1);
pt4(2)=-1*pt4(2);
pt4(3)=-1*pt4(3);

if pt4(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt4(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt4(1);         
end  


if pt4(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt4(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt4(2);    
      
end 

if pt4(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt4(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt4(3); 
end 

back_motor;

pt4(1)=-1*pt4(1);
pt4(2)=-1*pt4(2);
pt4(3)=-1*pt4(3);

display(pt4);

%pt5 conversion to machine command
if pt5(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt5(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt5(1);         
end  


if pt5(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt5(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt5(2);    
      
end 

if pt5(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt5(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt5(3); 
end 

run_motor;


pt5(1)=-1*pt5(1);
pt5(2)=-1*pt5(2);
pt5(3)=-1*pt5(3);

if pt5(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt5(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt5(1);         
end  


if pt5(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt5(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt5(2);    
      
end 

if pt5(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt5(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt5(3); 
end 


back_motor;

pt5(1)=-1*pt5(1);
pt5(2)=-1*pt5(2);
pt5(3)=-1*pt5(3);

display(pt5);


%pt6 conversion to machine command
if pt6(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt6(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt6(1);         
end  


if pt6(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt6(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt6(2);    
      
end 

if pt6(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt6(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt6(3); 
end 

run_motor;


pt6(1)=-1*pt6(1);
pt6(2)=-1*pt6(2);
pt6(3)=-1*pt6(3);

if pt6(1) >= 0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt6(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt6(1);         
end  


if pt6(2) >= 0

myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = pt6(2); 
        
else   
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = -1*pt6(2);    
      
end 

if pt6(3) >= 0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt6(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt6(3); 
end 

back_motor;

pt6(1)=-1*pt6(1);
pt6(2)=-1*pt6(2);
pt6(3)=-1*pt6(3);

display(pt6);


   

end



%% end of connection
COM_CloseNXT(COM_GetDefaultNXT());